package behavior;
import java.util.ArrayList;
import java.util.Iterator;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.proposal.RotatePilot;


public class WallAnalyser extends Analyser {
	private UltrasonicSensor sonicSensor;
	private Motor sensorMotor;
	private static int offset = 7;
	
	public WallAnalyser (){
		setSonicSensor(new UltrasonicSensor(SensorPort.S2));
		setSensorMotor(Motor.A);
		getSensorMotor().setSpeed(180);
	}	
	
	public int readValue(){
		ArrayList<Integer> vals = new ArrayList<Integer>();
		for(int i = 0; i < 100; i++){
			vals.add(getSonicSensor().getDistance());
		}
		int finalValue = vals.get(0);
		for(int value : vals){
			if (value != 255) finalValue = value;
		}
		return finalValue;
	}
	
	public ArrayList<Integer> analyseWalls(){
		sensorMotor.resetTachoCount();
		sonicSensor.getDistance(); //Hurr Ima sonicsensor Durr
		sleep(1000);
		ArrayList<Integer> distances = new ArrayList<Integer>(); 
		//getSonicSensor().getDistance()
		sensorMotor.rotate(90); sleep(80);
		distances.add(readValue());
		sensorMotor.rotate(-90); sleep(80);
		distances.add(readValue());
		sensorMotor.rotate(-90); sleep(80);
		distances.add(readValue());
		sensorMotor.rotate(90);
//		LCD.clear();
		System.out.println("Links:" + distances.get(0));
		System.out.println("Voor:" + distances.get(1));
		System.out.println("Rechts:" + distances.get(2));
		
		sensorMotor.rotate(-sensorMotor.getTachoCount());
		return distances;
	}

	public String analyseValues(ArrayList<Integer> distances){
		String sector = "";
		for(int value: distances){
			if(value < 60 && value >= 38 - offset){
				sector += "m"; //Muur
			}else if(value <= 23 + offset){
				sector += "v"; //Versmalling
			}else{
				sector += "i"; //Infinity
			}			
		}
		return sector;		
	}
	
	public int analyse(){
		ArrayList<Integer> distances = analyseWalls();
		String input = analyseValues(distances);
		int result=0;
		if(input.equals("viv"))
			return 8;
		//getInMiddle(distances);
		//miv en vim zijn ook straight panelen, maar de robot zit dicth langs 1 kant
		if(input.equals("mim")||input.equals("vim")||input.equals("miv"))  
			result= 1;
		else if(input.equals("imm") || input.equals("imv") || input.equals("ivm")||input.equals("ivv"))
			result= 3;
		else if(input.equals("mmi") || input.equals("mvi") || input.equals("vmi")||input.equals("ivv")) 
			result=6;
		return result;
		
	}
	public Motor getSensorMotor() {
		return sensorMotor;
	}

	private void setSensorMotor(Motor sensorMotor) {
		this.sensorMotor = sensorMotor;
	}

	public UltrasonicSensor getSonicSensor() {
		return sonicSensor;
	}

	private void setSonicSensor(UltrasonicSensor sonicSensor) {
		this.sonicSensor = sonicSensor;
	}
	
	public String getSectorDescription(String input){
		if(input.equals("mim")) return "Recht stuk";
		if(input.equals("imm")) return "Draai links";
		if(input.equals("mmi")) return "Draai rechts";
		if(input.equals("viv")) return "Versmalling";
		if(input.equals("iii")) return "Vlakte";
		return "Fout";
	}	
	public void test(){
		while(true){
			Button.waitForPress();
			System.out.println(analyse());
		}
	}
//	public void straighten(){
//		sensorMotor.rotate(90);
//		int left=readValue();
//		sensorMotor.rotate(-90);
//		if(left<80){
//			float angle=getAngle(left, true);
//			pilot.rotate((float)(angle*180/Math.PI));
//		}
//		else {
//			sensorMotor.rotate(-90);
//			int right=readValue();
//			sensorMotor.rotate(90);
//			if(right<80){
//				float angle=getAngle(left, false);
//				pilot.rotate((float)(angle*180/Math.PI));
//			}
//		}
//	}
//	public void getInMiddle(ArrayList<Integer> distances){
////		straighten();
//		int left = distances.get(0);
//		int right = distances.get(2);
//		if(left < 40){
////			float angle = getAngle(left,true);
////			pilot.rotate((float)(angle*180/Math.PI));
//			sensorMotor.rotate(90);
//			left=readValue();
//			sensorMotor.rotate(-90);
//			pilot.rotate(90);
//			pilot.travel(-(37-left));
//			pilot.rotate(-90);
//		}
//		else if(left>40 && left <70){
////			float angle = getAngle(left,true);
////			pilot.rotate((float)(angle*180/Math.PI));
//			sensorMotor.rotate(90);
//			left=readValue();
//			sensorMotor.rotate(-90);
//			pilot.rotate(-90);
//			pilot.travel(-(left-37));
//			pilot.rotate(90);
//		}
//
//		else if(right<40){
////			float angle = getAngle(right, false);
////			pilot.rotate(angle);
////			pilot.rotate(-(float)(angle*180/Math.PI));
//			sensorMotor.rotate(-90);
//			left=readValue();
//			sensorMotor.rotate(90);
//			pilot.rotate(-90);
//			pilot.travel(-(37-right));
//			pilot.rotate(90);
//		}
//		else if(right>40&& right<70){
////			float angle = getAngle(right, false);
////			pilot.rotate(-(float)(angle*180/Math.PI));
//			sensorMotor.rotate(90);
//			right=readValue();
//			sensorMotor.rotate(-90);
//			pilot.rotate(90);
//			pilot.travel(-(-37+right));
//			pilot.rotate(-90);
//		}
//		int forward = distances.get(1);
//		if(forward>40&&forward<70){
//			forward=readValue();
//			pilot.travel(forward-37);
//		} 
//		else if(forward<40){
//			pilot.travel(37-forward);
//		}
//		
//		
//	}
	
//	private float getAngle(int distance,boolean left){
//		int sign= 1;
//		if(!left)
//			sign=-1;
//		int distanceLeft1=distance;
//		pilot.travel(5);
//		sensorMotor.rotate(sign*90);
//		int distanceLeft2=readValue();
//		sleep(100);
//		distanceLeft2=readValue();
//		sensorMotor.rotate(sign*-90);
//		pilot.travel(-5);
//		double gamma = 22.5*Math.PI/180; // 180/pi is origineel
//		double v = (distanceLeft2-distanceLeft1);
//		float alpha1 =  (float) Math.atan(v/5);
//		double cos = Math.cos(Math.PI/2 - gamma);
//		float alpha2 = (float) ((v - 5*cos)/Math.sqrt(Math.pow((5*cos),2)  + 25 ));
//		if (alpha1 == 0)
//			return 0;
//		if(Math.abs(alpha1) < gamma)
//		{
//			//pilot.rotate(alpha2);
//			return alpha1;
//		}
//		return alpha2;
//	}
//	public void testMiddle(){
//		while(true){
//			straighten();
//			Button.waitForPress();
//			System.out.println(analyse());
//			Button.waitForPress();
//		}
//	}

}
